#include <io.h>

void waitms(int time) {
  BCSCTL1=0x00; //Sets ACLK=LFXT1CLK
  TACCR0=time*4; //assumes one run of clock=0.25ms
  TACTL=0x01D2; //Resets timer, sources from ACLK/8, and sets to count up to TACCR0
  while (!(TACTL & 1));
}

int main( void )
{
  unsigned char Plow, count;

  // Stop watchdog timer to prevent time out reset
  WDTCTL = WDTPW + WDTHOLD;
  
  /* Set P4.6 low to enable +5V_SW power */
  P4SEL &= ~0x40; /* P4.6 to 3.3V I/O */
  P4DIR |=  0x40; /* P4.6 to output */
  P4OUT &= ~0x40; /* P4.6 to 0 */

  P5DIR=0xF0;
  P5OUT|=0x80; //Set P5.7 high to reset camera
  waitms(100);
  P5OUT=0x00;

  BCSCTL2 |= SELS; //Set SMCLK to XT2 oscillator
  BCSCTL2 &= ~0x06; //Set clock divider to 1

  P3DIR=255; //Set P3 as output
  P3SEL=255; //Set P3 to alternate function

  U0CTL |= SYNC + I2C; //Set USART 0 to I2C
  U0CTL |= MST; //Make I2C master

  U0CTL &= ~(I2CEN); //Clear I2C enable
  I2CTCTL |= 0x10; //Set I2C to use ACLK
  I2CTCTL &= ~(I2CRM); //Set repeat mode to zero
  I2CTCTL &= ~(I2CWORD); //Set to byte mode
  I2CPSC = 0x03; //Set prescalar multiplier to 4x
  I2CSCLH = 0xA; //Set SCL high period to 12x prescalar period
  I2CSCLL = 0xA; //Set SCL low period to 12x prescalar period
  I2COA = 0x01; //Set own address to 1
  I2CSA = 0x42; //Set slave address
  U0CTL |= I2CEN; //Renable I2C

  waitms(100); //Wait for I2C to be ready

  I2CTCTL |= I2CTRX; //Set transmit mode
  I2CNDAT = 0x1; //Set to transmit one byte
  I2CDRB = 0x55; //Gives a data byte
  I2CTCTL |= I2CSTP; //Sets STOP condition bit
  I2CTCTL |= I2CSTT; //Initiates a start condition

  P1DIR=255;
  P2DIR=0;
  
  count=0;
  Plow=1;
  while(1) {
    waitms(1);
    P1OUT=I2CDCTL;
    
    if((P2IN & 1) == 0 && Plow == 0) {
      Plow = 1;
      count++;
    }
    else {
      Plow = 0;
    }
    
    if(count == 9) {
      P2DIR = 1;
      P2OUT = 0;
      waitms(1);
      P2DIR = 0;
      P2OUT = 0;
      count = 0;
    }
  }
}
